#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>
#include <yaml-cpp/yaml.h>

struct Coordinate {
    double x;
    double y;
    double yaw;
};

/**
 * @brief 获取 pgm 图片的高度
 */
int getPGMHeight(const std::string& filename)
{
    std::ifstream file(filename, std::ios::binary);
    if (!file.is_open()) {
        std::cerr << "无法打开文件: " << filename << std::endl;
        return -1;
    }

    std::string magicNumber;
    file >> magicNumber;

    // 跳过注释行
    std::string line;
    while (file.peek() == '#') {
        std::getline(file, line);
    }

    int width, height, maxValue;
    file >> width >> height >> maxValue;

    file.close();
    return height;
}

/**
 * @brief 解析的到 3d 坐标导航点
 * @param filename Go2 导航点配置文件名
 * @return 坐标列表
 */
std::vector<Coordinate> parseCoordinates(const std::string& filename)
{
    std::vector<Coordinate> coordinates;
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "Failed to open the file!" << std::endl;
        return coordinates;
    }

    std::string line;
    while (std::getline(file, line)) {
        std::istringstream iss(line);
        int index;
        double col1, col2, col3, col4;

        // 跳过第一列
        iss >> index;

        // 读取第二列 (x)
        iss >> col1;

        // 读取第三列 (y)
        iss >> col2;

        // 跳过第四列
        iss >> col3;

        // 读取第五列 (yaw)
        iss >> col4;

        Coordinate coord;
        coord.x = col1;
        coord.y = col2;
        coord.yaw = col4;
        coordinates.push_back(coord);
    }

    file.close();
    return coordinates;
}

int main(int argc, char* argv[])
{
    /* 解析 node.txt 得到 3d 坐标 */
    std::string nodefile = "Make_Graph_node.txt";
    std::vector<Coordinate> coords = parseCoordinates(nodefile);
    if (coords.empty()) {
        std::cerr << "No valid coordinate data was parsed." << std::endl;
        return -1;
    }

    /* 解析 yaml 得到分辨率和偏移量 */
    std::string yamlfile = "map.yaml";
    YAML::Node config = YAML::LoadFile(yamlfile);
    double resolution = config["resolution"].as<double>();
    YAML::Node originNode = config["origin"];
    std::vector<double> origin;
    for (size_t i = 0; i < originNode.size(); ++i) {
        origin.push_back(originNode[i].as<double>());
    }

    /* 解析得到 PGM 图片高度 */
    std::string pgmfile = "map.pgm";
    int height = getPGMHeight(pgmfile);

    /* 打印解析的结果 */
    std::cout << "resolution: " << resolution << std::endl;
    std::cout << "origin[0] : " << origin[0] << std::endl;
    std::cout << "origin[1] : " << origin[1] << std::endl;
    std::cout << "height    : " << height << std::endl;

    /* 坐标转换 */
    for (const auto& coord : coords) {
        /* 打印 3D 坐标点 */
        // std::cout << "x: " << coord.x << ", y: " << coord.y << ", yaw: " << coord.yaw << std::endl;

        /* 3D 雷达坐标系转 PGM 图片像素坐标 */
        int pgm_x = int((coord.x - origin[0]) / resolution);
        int pgm_y = height - int((coord.y - origin[1]) / resolution);
        float pgm_a = 0 - coord.yaw;

        /* 打印转化后的 2d 像素坐标 */
        std::cout << "x: " << pgm_x << ", y: " << pgm_y << ", yaw: " << pgm_a << std::endl;
    }

    return 0;
}